#include "imu.h"
#include "tinybus.h"
#include "intf_imu.h"

#include "cmsis_os.h"
#include "MahonyAHRS.h"

#include "stdio.h"
#include "BMI088.h"

osThreadId IMUtaskHandle;

static INTF_IMU_MessageTypeDef *imu_pubulish;

static float ins_gyro[3] = {0.0f};
static float gyro_offset_mid[3][100] = {0.0f};
static float gyro_offset[3] = {0};
static float accel_offset[3] ={0};
static float ins_accel[3] = {0.0f};
static float ins_magnet[3] = {0.0f};

static float ins_quat[4] = {1.0f, 0.0f, 0.0f, 0.0f};
static float ins_angel[3] = {0.0f};
static float temperate[1] = {0.0f};

static int TaskFlag[4] = {0} ;

//计算欧拉角
static void get_angle(float angle[3]);

void Bsp_imu_Mainloop()
{
eTaskState  TaskState;


    int count = 0;

    __disable_irq();

    // 初始化BMI088  (内置循环,因为中断而错乱的初始化会回头修复) taskflag留作以后有需求使用
    bmi088_init_jxust(TaskFlag[0]);
    // 初始化IST8310
    while (ist8310_init())
        ;
    __enable_irq();

    while (1)
    {
        TaskState = eTaskGetState(IMUtaskHandle);

        if((TaskFlag[2]&0b010)!=0b010)
        {
            temperate[0] = get_BMI088_temperate();
            bmi088_read_data_accel(ins_accel, temperate);
            TaskFlag[2] |= 0b010;
        }

        if((TaskFlag[2]&0b001)!=0b001)
        {
            // rad/s m/s^2 同时转置坐标系
            bmi088_read_data_gyro(ins_gyro,temperate);
            TaskFlag[2]|=0b01;
        }

        if((TaskFlag[2]&0b100)!=0b100)
        {
            //官方也没有把这一层中断关系写出来，只是简单地弃用，尝试写中断
            //  μT单位 转gs 包含减去mag_offset
            ist8310_read_mag(ins_magnet);
//            // 转回mT单位进行姿态解算*bias  得到最终的磁力计数据
            ist8310_calibrate_mag(ins_magnet); // 手动校准后投入数据在内部
            TaskFlag[2] |= 0b100;
        }

        if(TaskFlag[2]==0b111)
        {
            TaskFlag[2]=0b000;

            // TODO:need update status and time?
            imu_pubulish->status = TaskState;
            imu_pubulish->time = 0;

            if (temperate[0] - 40.0f > 5.5f || temperate[0] - 40.0f < -5.5f) {  //45

                BMI088_temp_control(temperate[0]);

            } else
            {
                // todo 数据量偏少,在flash模块加入后可设在flash当中
                if (TaskFlag[1] == 0) {
                    gyro_offset_mid[0][count] = ins_gyro[0];
                    gyro_offset_mid[1][count] = ins_gyro[1];
                    gyro_offset_mid[2][count] = ins_gyro[2];
                    count++;

                    if (count == 100) {
                        TaskFlag[1] = 1;
                    }
                } else if(TaskFlag[1]==1) {

                    for (int i = 0; i < 100; i++) {
                        gyro_offset[0] += gyro_offset_mid[0][i];
                        gyro_offset[1] += gyro_offset_mid[1][i];
                        gyro_offset[2] += gyro_offset_mid[2][i];
                    }
                    accel_offset[0]=ins_accel[0];
                            accel_offset[1]=ins_accel[1];
                                    accel_offset[2]=ins_accel[2];

                    gyro_offset[0] /= 100.0f;
                    gyro_offset[1] /= 100.0f;
                    gyro_offset[2] /= 100.0f;

                    TaskFlag[1]=2;   //offset计算完毕
                }

            // 应用所得单位计算得出四元数
            MahonyAHRSupdate(ins_gyro[0], ins_gyro[1], ins_gyro[2],
                             ins_accel[0], ins_accel[1], ins_accel[2],
                             ins_magnet[0], ins_magnet[1], ins_magnet[2], ins_quat);

            // 根据四元数得到弧度制欧拉角 将弧度制转化为角度制(*57.29577f)
            get_angle(ins_angel);

            for (uint8_t i = 0; i < 3; i++) {
                imu_pubulish->gyro[i] = ins_gyro[i] - gyro_offset[i];
                imu_pubulish->quat[i] = ins_quat[i];
                imu_pubulish->euler_angle[i] = ins_angel[i];
                imu_pubulish->accel[i] = ins_accel[i]; // 加速度校准问题不大,使用场景不多,可通过调试优化
            }
            }
        }
        //频率影响四元数更新速度，影响实际角度变化率
        osDelay(1);
    }

}

void bsp_imu_init(void)
{
    imu_pubulish = Bus_SharePtr(BSP_IMU_DATA_NAME, sizeof(INTF_IMU_MessageTypeDef));

    osThreadDef(IMU_Mainloop_task, Bsp_imu_Mainloop, osPriorityLow, 0, 256);
    IMUtaskHandle = osThreadCreate(osThread(IMU_Mainloop_task), NULL);
}

void get_angle(float angle[3]) {


    //官方采用的是左手系的方法解算出欧拉角 我们采用右手系的方法
    //1.0f yaw pitch roll

//    angle[0] = atan2f(2.0f*(q0*q3+q2*q1),2.0f*(q1*q1+q0*q0)-1.0f)* 57.295779513f;
//    angle[1] = asinf(-2.0f*(q3*q1-q0*q2))* 57.295779513f;
//    angle[2] = atan2f(2.0f*(q0*q1+q2*q3),2.0f*(q0*q0+q3*q3)-1.0f)* 57.295779513f;

//roll即为绕X轴数据,欧拉角正负讨论由坐标系指向方向逆时针为正(右手系逆时针为正,左手系顺时针为正)
//pitch即为绕Y轴旋转欧拉角,使轴指向面朝方向顺时针方向为正,也即是朝R标上方转为正
//yaw即为绕Z轴旋转欧拉角, 从上方看则为顺时针为正

//yaw数据与磁力计紧密相关,不使用磁力计无法确定稳定确定yaw数据(数据会处于波动状态)

    //yaw pitch roll   atanf会导致零点缺失问题

    angle[0] = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f -  2.0f*(q2 * q2 + q3 * q3)) * 57.29577f;

    angle[1] = asinf(2.0f * (q0 * q2 - q1 * q3)) * 57.29577f;

    angle[2] = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f-  2.0f*(q1 * q1 + q2 * q2)) * 57.29577f;

    //变弧度制
    {
//   angle[0] = atan2f(2.0f * (q0 * q1 + q2 * q3), 1.0f - 2.0f * (q1 * q1 + q2 * q2)) ;
//
//   angle[1] = asinf(2.0f * (q0 * q2 - q1 * q3)) ;
//
//   angle[2] = atan2f(2.0f * (q0 * q3 + q1 * q2), 1.0f - 2.0f * (q2 * q2 + q3 * q3)) ;
    }
}